#ifndef SWITCHER_HPP
#define SWITCHER_HPP

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <errno.h>
#include <string.h>
#include <wiringPi.h>
#include <vector>
#include <map>
#include <iostream>
#include <memory>
#include "rclcpp/rclcpp.hpp"

#define RAISE_FORWARD 21
#define RAISE_ENABLE 22
#define EXTEND_FORWARD 23
#define EXTEND_ENABLE 27
#define ROTATE_LEFT 24
#define ROTATE_RIGHT 28
#define LIFT_UP 25
#define LIFT_DOWN 29

class GpioSwitcher
{
private:
    std::map<int, int> gpio_states_;
    std::vector<int> pin_list;
public:
    GpioSwitcher(std::vector<int> gpio_num);
    ~GpioSwitcher();
    int switch_to(std::map<int, int> new_gpio_states_);
};

GpioSwitcher::GpioSwitcher(std::vector<int> gpio_pin_list)
{
    wiringPiSetup();
    pin_list = gpio_pin_list;
    for(auto gpio_pin : gpio_pin_list)
    {
        pinMode(gpio_pin, OUTPUT);
        digitalWrite(gpio_pin, 0);
        gpio_states_.insert(std::pair<int, int>{gpio_pin, 0});
    }
    RCLCPP_INFO(rclcpp::get_logger("GPIO SWITCH"), "Switcher init successed.");
}

GpioSwitcher::~GpioSwitcher()
{
    for(auto pin : pin_list)
    {
        pinMode(pin, INPUT);
    }
    printf("Exit successed.");
}

int GpioSwitcher::switch_to(std::map<int, int> new_gpio_states_)
{
    for(auto state_pair : new_gpio_states_)
    {
        if(state_pair.second != (gpio_states_[state_pair.first]))
        {
            gpio_states_[state_pair.first] = state_pair.second;
            digitalWrite(state_pair.first, state_pair.second);
            RCLCPP_INFO(rclcpp::get_logger("GPIO SWITCH"), "Change gpio %d state from %d to %d.", state_pair.first, gpio_states_[state_pair.first], state_pair.second);
            //std::cout << "Change gpio " << state_pair.first << " state from " << gpio_states_[state_pair.first] << " to " << state_pair.second << std::endl;
        }
    }
    return 0;
}

#endif
